#include "Chassis.h"

#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

#include "stdio.h"
#include "DJI_motor.h"
#include "mainc.h"
#include "controller.h"
#include "ARM.h"
PIDInstance Chassis1[2];
PIDInstance Chassis2[2];
//PIDInstance CLAW5[2];
float CHout1[2], CHout2[2];
//float fClaw_out[2];
PID_Init_Config_s Chassis1_pid[2] = {
    {.Kp = 1.0,
     .Ki = 15,
     .Kd = 0.1,
     .MaxOut = 700,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 5},
    {.Kp = 8,
     .Ki = 1,
     .Kd = 0.01,
     .MaxOut = 10000,
			
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s Chassis2_pid[2] = {
    {.Kp = 1.0,
     .Ki = 15,
     .Kd = 0.1,
     .MaxOut = 700,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 5},
    {.Kp = 8,
     .Ki = 1,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};

int32_t Center_pos = 0; // 中心位置
uint16_t center_tim[2] = {0};
//int32_t End_pos = -20050;
int32_t End_pos = -19570;
//int32_t End_pos = -10000;
int32_t End_pos2 = 5550;
int32_t End_pos21 = 5550;
int8_t chassis_flag1=0;
int8_t chassis_flag2=0;
//int16_t tim[5]={0,2,2,2,2};
float bili = 0.5;
int32_t ups_pos=1000;
uint8_t x = 0;
void ChassisPIDInit(void);
void CMoto_Zero(void); // 角度归零
// void is_break(void);    // 模式切换
void Chassis_clc(void); // pid计算

void Chassis(void *argument)
{
    vTaskDelay(1000);
    uint32_t current_time;
    ChassisPIDInit();
    CMoto_Zero();
    for (;;)
    {
        current_time = xTaskGetTickCount();
        if (CMode == 0)
        {
            CMoto_Zero();
            // Chassis_Motor[0].tx_data = 0;
            // Chassis_Motor[1].tx_data = 0;
        }
        else if (CMode == 1)
        {		
					
						if(x==0){
							RxData[1] = RxData[0];
							x=1;
						}
            Chassis_Motor[0].target_angle = Center_pos;
            Chassis_Motor[1].target_angle = -Center_pos; // 速度环
            
            int32_t err1, err2;
            err1 = Chassis_Motor[0].target_angle - Chassis_Motor[0].totall_angle;
            err2 = Chassis_Motor[1].target_angle - Chassis_Motor[1].totall_angle;
						
					
            if (err2 < 10 && err1 < 10 && err1 > -10 && err2 > -10)
            {
                center_tim[0]++;
                if (center_tim[0] > 1)
                {		
										//Center_pos = -Center_pos;
                    CMode++;
                    center_tim[0] = 0;
                }
            }
        }
        else if (CMode == 4)	//6
        {		
						/*
						if (Chassis_Motor[0].rpm<-3150||Chassis_Motor[0].rpm>3150){
							Chassis1[0].MaxOut=4000;
							Chassis2[0].MaxOut=4000;
						}
						
						if (Chassis_Motor[0].rpm<-2450||Chassis_Motor[0].rpm>2450){
							Chassis1[0].MaxOut=3200;
							Chassis2[0].MaxOut=3200;
						}
						*/
						if (Chassis_Motor[0].rpm<-3450||Chassis_Motor[0].rpm>3450){
							Chassis1[0].MaxOut=4000;
							Chassis2[0].MaxOut=4000;
						}
						
						else if (Chassis_Motor[0].rpm<-2950||Chassis_Motor[0].rpm>2950){
							Chassis1[0].MaxOut=3500;
							Chassis2[0].MaxOut=3500;
						}
						
						else if ((Chassis_Motor[0].rpm<-2450)||(Chassis_Motor[0].rpm>2450)){
							Chassis1[0].MaxOut=3000;
							Chassis2[0].MaxOut=3000;
						}
						
						else if ((Chassis_Motor[0].rpm<-1950)||(Chassis_Motor[0].rpm>1950)){
							Chassis1[0].MaxOut=2500;
							Chassis2[0].MaxOut=2500;
							//tim[4]--;
						}
						
						else if ((Chassis_Motor[0].rpm<-1450)||(Chassis_Motor[0].rpm>1450)){
							Chassis1[0].MaxOut=2000;
							Chassis2[0].MaxOut=2000;
							//tim[3]--;
						}
						
						else if ((Chassis_Motor[0].rpm<-950)||(Chassis_Motor[0].rpm>950)){
							Chassis1[0].MaxOut=1500;
							Chassis2[0].MaxOut=1500;
							//tim[2]--;
						}
						
						else if (Chassis_Motor[0].rpm<-650||Chassis_Motor[0].rpm>650){
							Chassis1[0].MaxOut=1000;
							Chassis2[0].MaxOut=1000;
							//tim[0]--;
						}
						
            Chassis_Motor[0].target_angle = End_pos2;
            Chassis_Motor[1].target_angle = -End_pos21; // 速度环
                                                       // Chassis_clc();
                                                       // Chassis_Motor[0].tx_data = CHout1[1];
                                                       // Chassis_Motor[1].tx_data = CHout2[1];
            int32_t err1, err2;
            err1 = Chassis_Motor[0].target_angle - Chassis_Motor[0].totall_angle;
            err2 = Chassis_Motor[1].target_angle - Chassis_Motor[1].totall_angle;
            if (err1 < 800 && err2 < 800 && err2 > -800 && err1 > -800)
            {
                center_tim[0]++;
                if (center_tim[0] > 300)
                {
                    chassis_flag1=1;//CMode++;
										//CMode++;
                    center_tim[0] = 0;
                }
            }
					
        }
        else if (CMode == 7)	//10
        {		
						//if(Chassis_Motor[1].totall_angle>-End_pos*0.3)
						//{
						//bili = Chassis_Motor[1].totall_angle/-End_pos;
							//Chassis1[0].MaxOut=10000*(1-bili);
							//Chassis2[0].MaxOut=10000*(1-bili);
						//}
						
						//else
						//{
						Chassis1[0].MaxOut=10000;
						Chassis2[0].MaxOut=10000;	
						//}
//						if(Chassis_Motor[0].target_angle<End_pos)
//						{
//							Chassis_Motor[0].target_angle+=100;
//						}
//						else
//						{
//							Chassis_Motor[0].target_angle=End_pos;
//						}
//						if(Chassis_Motor[1].target_angle>-End_pos)
//						{
//							Chassis_Motor[1].target_angle-=100;
//						}
//						else
//						{
//							Chassis_Motor[1].target_angle=-End_pos;
//						}
            Chassis_Motor[0].target_angle = End_pos;
            Chassis_Motor[1].target_angle = -End_pos;
            // Chassis_clc(); // 速度环
            // Chassis_Motor[0].tx_data = CHout1[1];
            // Chassis_Motor[1].tx_data = CHout2[1];
            // is_break();
            int32_t err1, err2;
            err1 = Chassis_Motor[0].target_angle - Chassis_Motor[0].totall_angle;
            err2 = Chassis_Motor[1].target_angle - Chassis_Motor[1].totall_angle;
            if (err2 < 800 && err1 < 800 && err1 > -800 && err2 > -800)
            {
                center_tim[0]++;
                if (center_tim[0] > 200)
                {
                    chassis_flag2=1;//CMode++;
                    center_tim[0] = 0;
                }
            }
        }
				else if (CMode == 10)	//13
        {
            Chassis1[0].MaxOut=4000;
						Chassis2[0].MaxOut=4000;
						Chassis_Motor[0].target_angle = 0;
            Chassis_Motor[1].target_angle = 0;
						
        }
        if (CMode == 0)
        {
            Chassis_Motor[0].tx_data = 0;
            Chassis_Motor[1].tx_data = 0;
        }
        else
        {
            Chassis_clc(); // 速度环
            Chassis_Motor[0].tx_data = CHout1[1];
            Chassis_Motor[1].tx_data = CHout2[1];
						//Claw_Motor[4].tx_data = 0;
        }
        Chassis_Send();
        vTaskDelayUntil(&current_time, 4); // 绝对延时
    }
}

void CMoto_Zero(void) // 角度归零
{
    Chassis_Motor[0].offset_angle = Chassis_Motor[0].angle;
    Chassis_Motor[0].round = 0;
    Chassis_Motor[1].offset_angle = Chassis_Motor[1].angle;
    Chassis_Motor[1].round = 0;

}

void ChassisPIDInit(void) // pid初始化
{
    PIDInit(&Chassis1[0], &Chassis1_pid[0]);
    PIDInit(&Chassis1[1], &Chassis1_pid[1]);

    PIDInit(&Chassis2[0], &Chassis2_pid[0]);
    PIDInit(&Chassis2[1], &Chassis2_pid[1]);

}

void Chassis_clc(void)
{
    CHout1[0] = PIDCalculate(&Chassis1[0], Chassis_Motor[0].totall_angle, Chassis_Motor[0].target_angle); // 位置环
    CHout1[1] = PIDCalculate(&Chassis1[1], Chassis_Motor[0].rpm, CHout1[0]);                              // 速度环
    CHout2[0] = PIDCalculate(&Chassis2[0], Chassis_Motor[1].totall_angle, Chassis_Motor[1].target_angle); // 位置环
    CHout2[1] = PIDCalculate(&Chassis2[1], Chassis_Motor[1].rpm, CHout2[0]);

}

// void is_break(void)
// {
//     if (CHout1[1] < 500 && CHout1[1] > -500)
//     {
//         center_tim[0]++;
//     }
//     if (CHout2[1] < 500 && CHout2[1] > -500)
//     {
//         center_tim[1]++;
//     }
//     if (center_tim[0] > 800 && center_tim[1] > 800)
//     {
//         center_tim[0] = 0;
//         center_tim[1] = 0;
//         CMode++;
//     }
// }